#!/usr/bin/python3
# -*- coding: utf-8 -*-
# 智能小车感官

import RPi.GPIO as GPIO
import time

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
#定义引脚
#舵机
Pin_Servo = 21
#超声波
Pin_Echo = 16
Pin_Trig = 20

#舵机pwm
p_servo = None


#通过超声波测距出的结果：前方180度内的障碍物距离
ObstacleDistance = None

#障碍物距离暂存list来初始化
def init(od = [0 for i in range(19)]):
    #初始化舵机
    GPIO.setup(Pin_Servo, GPIO.OUT, initial=False)
    global ObstacleDistance, p_servo
    ObstacleDistance = od
    p_servo = GPIO.PWM(Pin_Servo, 50)
    p_servo.start(0)
    #初始化超声波传感器
    GPIO.setup(Pin_Trig, GPIO.OUT, initial = GPIO.LOW)
    GPIO.setup(Pin_Echo, GPIO.IN)

#超声波测距
def checkDist():
    GPIO.output(Pin_Trig, GPIO.HIGH)
    time.sleep(0.00015)
    GPIO.output(Pin_Trig, GPIO.LOW)
    while not GPIO.input(Pin_Echo):
        pass
    t1 = time.time() 
    while GPIO.input(Pin_Echo):
        pass
    t2 = time.time()
    return (t2-t1)*340*100/2
    
#舵机的几种运行状态
#1. 扫描一遍左边90度，从左扫到中间，最终指向正前方
def scanLeft():
    for i in range(180, 89, -10):
        p_servo.ChangeDutyCycle(2.5 + 10 * i / 180)
        #把当前距离填入列表
        ObstacleDistance[18-int(i/10)] = checkDist() - 8
        time.sleep(0.03)
    
#2. 扫描一遍右边90度，从右扫到中间，最终指向正前方
def scanRight():
    for i in range(0, 91, 10):
        p_servo.ChangeDutyCycle(2.5 + 10 * i / 180)
        #把当前距离填入列表(减掉一个超声波距离车边缘的距离和一个安全距离)(单位：cm)
        ObstacleDistance[18-int(i/10)] = checkDist() - 8
        time.sleep(0.03)

#3. 右(180度)-->左(0度)-->右(180度)旋转，扫描前方180度
def scanArount():
    #从右往左
    for i in range(0, 181, 10):
        p_servo.ChangeDutyCycle(2.5 + 10 * i / 180)
        #给舵机一段时间转到指定位置
        time.sleep(0.1)
        #测距并把当前距离填入列表
        ObstacleDistance[18-int(i/10)] = checkDist() - 8
        
    #从左往右
    for i in range(180, -1, -10):
        p_servo.ChangeDutyCycle(2.5 + 10 * i / 180)
        #给舵机一段时间转到指定位置
        time.sleep(0.1)
        #把当前距离填入列表
        ObstacleDistance[18-int(i/10)] = checkDist() - 8

#4. 扫描指定角度距离(角度从左到右为：-90 ~ 90)
def scanByDirection(angle):
    if -90 > angle or 90 < angle:
        print("输入角度超出指定范围[-90, 90]")
        return
    p_servo.ChangeDutyCycle(2.5 + 10 * (angle+90) / 180)
    ObstacleDistance[18-int(i/10)] = checkDist() - 8
    time.sleep(0.03)

def clean():
    p_servo.stop()
    GPIO.cleanup()
    
#------test-----------------------
if __name__ == "__main__":
    try:
        ObstacleDistance = [0 for i in range(19)]
        init(ObstacleDistance)
        scanArount()
        print("扫描完成，前方180度内的障碍物距离分别为：")
        for i in ObstacleDistance:
            print(i)
    except KeyboardInterrupt:
        clean()
        print("捕捉异常后清理资源完成")
    else:
        clean()
        print("正常测试结束")